%% 最小方差调节器的仿真代码
% Written by Albert Wu
% git@gitee.com:albertwuoo/adaptive_-control.git
% 参考代码:https://zhuanlan.zhihu.com/p/656282259#

clc;
clear;

%系统相关的矩阵,其中F与G可通过长除法得到
A = [1, -0.8]; 
B = [1, 0.5]; 
d = 2; 
C = [1, 0.7];
F = [1, 1.5]; 
G = 1.2;

fs=0.01; %采样周期fs

Gc = -filt(G,B,fs)*filt(1,F,fs); %生成uk与yk之间的离散传递函数，函数原型为filt(numerator,denominator,ts)

na = numel(A)-1; %A的阶数，后续同理
nb = numel(B)-1; 
nc = numel(C)-1;
n_gc_n = numel(Gc.num{1}); 
n_gc_d = numel(Gc.den{1}); 

Nsim = 500; %采样点数为500个
y_seq = zeros(1,max(na,n_gc_n)); %产生0时刻前的y初始值，用于滤波计算
u_seq = zeros(1,d+numel(Gc.Denominator{1})); %产生0时刻前的u初始值，用于滤波计算
v_seq = zeros(1,nc);
for i = 1:Nsim
    v_seq = [v_seq, randn]; % 产生当前时刻的扰动，randn产生均值为0,方差为1的正态分布噪声
    Ayt = B*u_seq(end-d+1:-1:end-d+1-nb)' + C * v_seq(end:-1:end-nc)'; % 计算滑动平均后得到的A*yt
    yt = Ayt - A(2:end)*y_seq(end:-1:end-na+1)' ; %计算滑动平均前的yt
    y_seq = [y_seq,yt]; %将当前时刻yt加入到y_seq序列中
    
    BFut = Gc.num{1} * y_seq(end:-1:end-n_gc_n+1)'; %计算滑动平均后得到的B*F*ut = G*yk
    ut = BFut - Gc.den{1}(2:end)*u_seq(end:-1:end-n_gc_d+2)'; %计算滑动平均前的ut
    ut = ut/Gc.den{1}(1);
    u_seq = [u_seq,ut]; %将当前时刻ut加入到u_seq序列中
end

figure(1)
plot(1:Nsim,u_seq(d+numel(Gc.Denominator{1})+1:end),'r');
xlabel("采样点")
ylabel("控制量u")
legend("控制量u")
grid
title("控制曲线")

figure(2)
plot(1:Nsim,y_seq(max(na,n_gc_n)+1:end),'b');
xlabel("采样点")
ylabel("输出量y")
legend("输出量y")
grid
title("输出响应曲线")